/**
 * Copyright 2013 Douglas Lacher
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * @author Douglas Lacher <doug.lacher@gmail.com>
 */

package com.devioustoast.andmoco;

import android.util.Log;

/**
 * A Servo represents an individual servo in the Robot.  This class contains helpers for tracking
 * the servo's position and offset.
 *
 * @author Douglas Lacher <doug.lacher@gmail.com>
 */
public class Servo {

  public static final String LOG_TAG = "Servo";

  // Minimum and maximum pulse widths in micro-seconds.
  public static final int DEFAULT_SERVO_PULSE_MIN = 500;  // 500us
  public static final int DEFAULT_SERVO_PULSE_MAX = 2500;  // 2500us

  //Parameters: (int) Servo number.
  public static final String POMOCO_COMMAND_KILL_SERVO_TMPL = "#%dL";
  // Parameters: (int) Servo number, (int) Servo position in microseconds.
  public static final String POMOCO_COMMAND_MOVE_SERVO_TMPL = "#%dP%d";

  private BluetoothConnector mBluetoothConnector;
  private int mServoNumber;
  private int mPosition;
  private int mOffset;
  private int mPulseMax;
  private int mPulseMin;

  /**
   * This constructor uses the default pulse width minimum and maximum
   * (see DEFAULT_SERVO_PULSE_MIN and DEFAULT_SERVO_PULSE_MAX).
   *
   * @param bluetoothConnector a BluetoothConnector to use to communicate with this servo.
   * @param servoNumber this servo's number on the robot.
   * @param offset this servo's offset as a micro-second pulse width.
   */
  public Servo(BluetoothConnector bluetoothConnector, int servoNumber, int offset) {
    mBluetoothConnector = bluetoothConnector;
    mServoNumber = servoNumber;
    mPosition = 0;
    mOffset = offset;
    mPulseMin = DEFAULT_SERVO_PULSE_MIN;
    mPulseMax = DEFAULT_SERVO_PULSE_MAX;
  }

  /**
   * @param bluetoothConnector a BluetoothConnector to use to communicate with this servo.
   * @param servoNumber this servo's number on the robot.
   * @param offset this servo's offset as a micro-second pulse width.
   * @param servoPulseMin this servo's minimum valid pulse width.
   * @param servoPulseMax this servo's maximum valid pulse width.
   */
  public Servo(
      BluetoothConnector bluetoothConnector, int servoNumber, int offset, int servoPulseMin,
      int servoPulseMax) {
    mBluetoothConnector = bluetoothConnector;
    mServoNumber = servoNumber;
    mPosition = 0;
    mOffset = offset;
    mPulseMin = servoPulseMin;
    mPulseMax = servoPulseMax;
  }

  public int getServoNumber() {
    return mServoNumber;
  }

  public int getPosition() {
    return mPosition;
  }

  public int getOffset() {
    return mOffset;
  }

  public int getServoMinimumPulse() {
    return mPulseMin;
  }

  public int getServoMaximumPulse() {
    return mPulseMax;
  }

  /**
   * Sets a new position for this servo relative to its current position.  This servo's
   * offset and boundaries are still respected.
   *
   * @param microseconds the new position as a micro-second pulse.  Can be a positive or negative
   *        value.  The new position will be added to the current position.
   * @return the position that was set for this servo.  May differ from the requested position if
   *         this servo has an offset or the new position is out of bounds.
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public int setRelativePositionUsec(int microseconds) throws InterruptedException {
    return move(mPosition + microseconds + mOffset);
  }

  /**
   * Sets a new absolute position for this servo ignoring its current position.  This servo's
   * offset and boundaries are still respected.
   *
   * @param microseconds the new position as a micro-second pulse.
   * @return the position that was set for this servo.  May differ from the requested position if
   *         this servo has an offset or the new position is out of bounds.
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public int setAbsolutePositionUsec(int microseconds) throws InterruptedException {
    return move(microseconds + mOffset);
  }

  /**
   * Sets a new position for this servo relative to its current position.  This servo's
   * offset and boundaries are still respected.
   *
   * @param degrees the new position in degrees.  Can be a positive or negative value.  The
   *        new position will be added to the current position.
   * @return the position that was set for this servo.  May differ from the requested position if
   *         this servo has an offset or the new position is out of bounds.
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public int setRelativePositionDegrees(float degrees) throws InterruptedException {
    return move(mPosition + (int) (1500 + (degrees * 11.1111111)) + mOffset);
  }

  /**
   * Sets a new absolute position for this servo ignoring its current position.  This servo's
   * offset and boundaries are still respected.
   *
   * Note that 0 degrees is "center" or 1500 micro-seconds pulse-width.
   *
   * @param degrees the new position in degrees.
   * @return the position that was set for this servo.  May differ from the requested position if
   *         this servo has an offset or the new position is out of bounds.
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public int setAbsolutePositionDegrees(float degrees) throws InterruptedException {
    return move((int) (1500 + (degrees * 11.1111111)) + mOffset);
  }

  /**
   * Sets the servo's offset.
   *
   * @param newOffset the new offset as a micro-second pulse.
   * @return the offset that was set for this servo.
   */
  public int setOffset(int newOffset) {
    mOffset = newOffset;
    return mOffset;
  }

  /**
   * Centers this servo.
   *
   * @return this servo's center value.
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public int center() throws InterruptedException {
    return move(mPulseMax - ((mPulseMax - mPulseMin) / 2) + mOffset);
  }

  /**
   * Kills this servo.
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public void killServo() throws InterruptedException {
    Log.d(LOG_TAG, String.format("Killing servo %d.", mServoNumber));
    mBluetoothConnector.send(String.format(POMOCO_COMMAND_KILL_SERVO_TMPL, mServoNumber));
  }

  /**
   * Moves this Servo to the provided position.
   *
   * You probably want to use setRelativePosition or setAbsolutePosition instead of this method.
   *
   * @param microseconds the absolute position, specified in microseconds (e.g. 1500).
   * @return this servo's new position.
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public int move(int microseconds) throws InterruptedException {
    mPosition = restrictPulseWidth(microseconds);
    Log.d(LOG_TAG, String.format("Moving servo %d to position %d.", mServoNumber, microseconds));
    mBluetoothConnector.send(
        String.format(POMOCO_COMMAND_MOVE_SERVO_TMPL, mServoNumber, mPosition));
    return mPosition;
  }

  /**
   * Restricts the desired pulse width to this servo's minimum and maximum values.
   *
   * @param microseconds the desired pulse width.
   * @return a pulse width bounded by this servo's minimum and maximum values.
   */
  private int restrictPulseWidth(int microseconds) {
    if (microseconds < mPulseMin) {
      microseconds = mPulseMin;
    } else if (microseconds > mPulseMax) {
      microseconds = mPulseMax;
    }
    return microseconds;
  }
}
